#include <ros/ros.h>
#include <ros/time.h>
#include <unistd.h>
#include <data_record/Cmd.h>
#include <data_record/State.h>

#include <ant_msgs/ControlCmd2.h>
#include <ant_msgs/State.h>

using namespace std;

void CmdCallback(const ant_msgs::ControlCmd2::ConstPtr& msg);
void StateCallback(const ant_msgs::State::ConstPtr& msg);

string cmd_car = "cmd";
string state_car = "state";

data_record::Cmd cmd;
data_record::State state;

ros::Publisher  pub_cmd_;
ros::Publisher  pub_state_;

ros::Subscriber sub_cmd_;
ros::Subscriber sub_state_;

string cmd_speed_;
string state_speed_;

bool init(ros::NodeHandle& nh)
{
    cmd_speed_ = nh.param<std::string>("cmd", "/ControlCmd2");
    state_speed_ = nh.param<std::string>("state", "/State");
    
    sub_cmd_ = nh.subscribe(cmd_speed_, 10, &CmdCallback);
    sub_state_ = nh.subscribe(state_speed_, 10, &StateCallback);
    
    string cur_cmd_ = nh.param<std::string>("cur_cmd", "/cur_cmd");
    string cur_state_ = nh.param<std::string>("cur_state", "/cur_state");
    
    pub_cmd_ = nh.advertise<data_record::Cmd>(cur_cmd_, 10);
    pub_state_ = nh.advertise<data_record::State>(cur_state_, 10);

}

void CmdCallback(const ant_msgs::ControlCmd2::ConstPtr& msg)
{
    cmd.header.frame_id = cmd_car;
    cmd.header.stamp=ros::Time::now();
    cmd.speed = msg->set_speed;
    
    pub_cmd_.publish(cmd);
}

void StateCallback(const ant_msgs::State::ConstPtr& msg)
{
//    std::cout << 2222222 << std::endl;
    state.header.frame_id = state_car;
    state.header.stamp=ros::Time::now();
    state.speed = msg->vehicle_speed;
    
    pub_state_.publish(state);
}

int main(int argc, char **argv) {

    ros::init(argc, argv, "temp_node");
    ros::NodeHandle nh("~");
    init(nh);
    ros::spin();
    return 0;
}
